#include "rearwheel_feedback.h"

namespace controlNS {
fstream json_file("/home/hyh/coder/cpp/ros_simulink/src/config/rearwheel_controlParm.json");
nlohmann::json control_json = nlohmann::json::parse(json_file);
float kPI = myNumpy::kPI;
float kPhi = control_json.at("kPhi");  //定义rearwheel_feedback算法控制参数：横摆角偏差增益
float kE = control_json.at("kE");    //定义rearwheel_feedback算法控制参数：横向位置偏差增益
float kWheelBase = control_json.at("kWheelBase"); //定义车辆的轴距[m]
float kSteeringRatio = control_json.at("kSteeringRatio");   //车辆的传动比
float kMaxSteerAngle = myNumpy::deg2rad(control_json.at("kMaxSteerAngle"));   //车轮最大的转角[rad]


void State::updateFromPreScan(float x, float y, float yaw, float v) {
    this->x_ = x;
    this->y_ = y;
    this->yaw_ = myNumpy::NormalizeAngle(yaw);
    this->v_ = v;
};

CalcPath::CalcPath(const string &path_x, const string &path_y, const string &path_yaw, const State &state) {
    this->path_x_ = myNumpy::ReadPath(path_x);  //读取存储的路径信息
    this->path_y_ = myNumpy::ReadPath(path_y);
    this->path_yaw_ = myNumpy::ReadPath(path_yaw);

    this->calNearestPoint(state);   //计算最近点的索引
    this->calError(state);  //计算路径最近点和本车后轮中心之间的横向误差和航向误差
    this->calDiff1();   //计算路径上最近点处的一阶导数
    this->calDiff2();   //计算路径上最近点处的二阶导数
    this->calCurvature();   //计算路径上最近点处的曲率


}

void CalcPath::calNearestPoint(const State &state) {
    float min_distance = 0.0f;
    for (int i = 0; i < path_x_.size(); i++) {
        float dx = path_x_[i] - state.x_;
        float dy = path_y_[i] - state.y_;
        float ds = dx * dx + dy * dy;
        if (i == 0 || ds < min_distance) {
            min_distance = ds;
            this->min_distance_index_ = i;
        }
    }
}

void CalcPath::calDiff1() {
    int i = this->min_distance_index_;
    if (this->min_distance_index_ == path_x_.size() - 1)
        this->diff1_ = (path_y_[i] - path_y_[i - 1]) / (path_x_[i] - path_x_[i - 1]);   //泰勒展开式去掉二阶以后的项
    else
        this->diff1_ = (path_y_[i + 1] - path_y_[i]) / (path_x_[i + 1] - path_x_[i]);

}

void CalcPath::calDiff2() {
    int i = this->min_distance_index_;
    if (i > 1)  // 说明前面至少还有两个点，用向后微分公式(backward)
        this->diff2_ = (path_y_[i] - 2 * path_y_[i - 1] + path_y_[i - 2]) / pow((path_x_[i] - path_x_[i - 1]), 2);
    else
        this->diff2_ = (path_y_[i + 2] - 2 * path_y_[i + 1] + path_y_[i]) / pow((path_x_[i + 1] - path_x_[i]), 2);
}

void CalcPath::calCurvature() {
    float dy = this->diff1_;
    float ddy = this->diff2_;
    this->curvature_ = abs(ddy) / pow((1 + pow(dy, 2)), 3 / 2);    //曲率公式
}

void CalcPath::calError(const State &state) {
    int i = this->min_distance_index_;
    float cyaw = myNumpy::NormalizeAngle(path_yaw_[i]);   // 参考点的yaw angle
    float dx = state.x_ - path_x_[i];
    float dy = state.y_ - path_y_[i];
    this->cross_track_error_ = dx * sin(cyaw) - dy * cos(cyaw); //带符号的横向偏差
//    this->cross_track_error_ = sqrt(dx * dx + dy * dy);
    this->heading_track_error_ = myNumpy::NormalizeAngle(state.yaw_ - cyaw); //航向误差
//    if (heading_track_error_ < 0)
//        this->cross_track_error_ *= -1; //保持横向误差和航向误差同号
}

void CalcPath::calUpdate(const State &state) {
    this->calNearestPoint(state);   //计算最近点的索引
    this->calError(state);  //计算路径最近点和本车后轮中心之间的横向误差和航向误差
    this->calDiff1();   //计算路径上最近点处的一阶导数
    this->calDiff2();   //计算路径上最近点处的二阶导数
    this->calCurvature();   //计算路径上最近点处的曲率
}


float RearWheelFeedbackControl::calDelta(const State &state, const CalcPath &calcPath) {
    //定义一些符号,和后轮反馈控制算法公式中的符号保持一致
    float v_r = state.v_;   //车辆后轮的线速度[m/s]
    float k_s = calcPath.curvature_;    //参考点处的曲率
    float e = calcPath.cross_track_error_;  //参考点和本车后轮中心的横向位置误差[m]
    float theta_e = calcPath.heading_track_error_;  //参考点的切线角和本车横摆角的误差[deg]
    float omega = 0.0f; //车身横摆角速度[rad/s]
    cout << "[v_r, k_s, e, theta_e] = " << v_r << " " << k_s << " " << e << " " << theta_e << endl;
    /** 对于后轮反馈控制算法，为了保证车辆的李亚普洛夫稳定性，车身横摆角速度omega为
     *  其中，kPhi为横摆角偏差控制增益，kE为横向位置偏差反馈控制增益*/
    if (theta_e == 0)
        omega = 0.0f;
    else {
        omega = (v_r * k_s * cos(theta_e) / (1 - k_s * e)) - (kPhi * abs(v_r) * theta_e) -
                (kE * v_r * sin(theta_e) * e / theta_e);
    }

    cout << "k1 = " << kPhi << " k2 = " << kE << endl;
    cout << "omega = " << omega << endl;
    float delta = atan2(kWheelBase * omega, v_r);   //根据车辆的运动学方程
    this->delta_ = min(kMaxSteerAngle, max(delta, -kMaxSteerAngle));  //限定车轮转角的范围为[-kMaxSteerAngle, kMaxSteerAngle]
    return this->delta_;    //车轮转角[rad];
}
}//namespace controlNS
